#ifndef DISABLE_DYNAMIXEL
#ifndef __DYNAMIXEL_H__
#define __DYNAMIXEL_H__

#include <stdint.h>
#include "defs.h"

// AX-12 packet header bytes
#define DXL_HEADER1                   0xff
#define DXL_HEADER2                   0xff

// AX-12 commands
#define DXL_PING                      0x01
#define DXL_READ_DATA                 0x02
#define DXL_WRITE_DATA                0x03
#define DXL_REG_WRITE                 0x04
#define DXL_ACTION                    0x05
#define DXL_RESET                     0x06
#define DXL_SYNC_WRITE                0x83

// dynamixel error codes used in status packets
#define DXL_INSTRUCTION_ERROR         (1<<6)
#define DXL_OVERLOAD_ERROR            (1<<5)
#define DXL_CHECKSUM_ERROR            (1<<4)
#define DXL_RANGE_ERROR               (1<<3)
#define DXL_OVERHEATING_ERROR         (1<<2)
#define DXL_ANGLE_LIMIT_ERROR         (1<<1)
#define DXL_INPUT_VOLTAGE_ERROR       (1<<0)

// available AX-S1 baud rates
#define DXL_BAUD_RATE_9600            0xcf
#define DXL_BAUD_RATE_19200           0x67
#define DXL_BAUD_RATE_57600           0x22
#define DXL_BAUD_RATE_115200          0x10
#define DXL_BAUD_RATE_200000          0x09
#define DXL_BAUD_RATE_250000          0x07
#define DXL_BAUD_RATE_400000          0x04
#define DXL_BAUD_RATE_500000          0x03
#define DXL_BAUD_RATE_1000000         0x01

// maxumum time we wait for a reply
#define DXL_REPLY_TIMEOUT             5000

// response levels
#define DXL_RETURN_LEVEL_NEVER        0
#define DXL_RETURN_LEVEL_READ_ONLY    1
#define DXL_RETURN_LEVEL_ALWAYS       2


// dynamixel broadcast ID
#define DXL_BROADCAST_ID              0xfe

// important indices in the instruction/response packets
#define DXL_PACKET_ID_BYTE            2
#define DXL_PACKET_LENGTH_BYTE        3
#define DXL_PACKET_INSTRUCTION_BYTE   4
#define DXL_PACKET_ERROR_BYTE         4
#define DXL_PACKET_PARAMETER_1_BYTE   5


// packet lengths for different status packets
#define DXL_GET_POSITION_RESPONSE_LENGTH  8
#define DXL_PING_RESPONSE_LENGTH          6
#define DXL_DEFAULT_RESPONSE_LENGTH       4

// paramLength + 1 <= 255 (1 byte)
#define DXL_MAX_PARAMS        253

// container for a command or status packet
typedef struct dxl_packet {
    uint8_t header1;
    uint8_t header2;
    uint8_t id;
    uint8_t instrErr;        // the instruction for command packets, or error code for response packets
    uint8_t paramLength;
    uint8_t params[DXL_MAX_PARAMS];
    uint8_t checksum;
} DXL_Packet;


// send an arbitrary command to a servo
// if responseDest is not NULL the thread will wait until a status packet is received, and store the response in responseDest
void sendCommand(uint8_t id, uint8_t command, uint8_t parameters[], uint8_t paramLength, DXL_Packet *dest);
void writeByte(uint8_t id, uint8_t address, uint8_t data, DXL_Packet *dest);

// send specific commands to a servo
BOOL ping(uint8_t id);
void readBytes(uint8_t id, uint8_t startingAddress, uint8_t numBytes, DXL_Packet *dest);

// calculate and return a packet checksum
uint8_t calculateCheckSum(uint8_t id, uint8_t instruction, uint8_t const parameters[], uint8_t n);

// assign a checksum to a packet (also returns the checksum)
uint8_t createChecksum(DXL_Packet *packet);

// validate a response packet
BOOL validateResponsePacket(DXL_Packet *packet);


/*
void handleCommand(void *arg);
void handleReply(void *arg);
void motorOff(uint8_t id, uint8_t replyStatus);
void motorOn(uint8_t id, uint8_t replyStatus);
void setPositionAndSpeed(uint8_t id, uint8_t position, uint8_t speed, uint8_t replyStatus);
void moveLoadCheck(uint8_t id, uint8_t direction, uint8_t replyStatus);
*/


#endif // __DYNAMIXEL_H__
#endif
